A single-link pendulum with torque applied to the pivot point and no friction has the dynamics: $$ \dot{x} = \begin{bmatrix} x_2 \\ - \frac{g}{l} \sin(x_1) \end{bmatrix} + \begin{bmatrix} 0 \\ 1 \end{bmatrix}u $$

I've written a pendulum.py function to more easily work with our pendulum system. This function can be used to understand the passive dynamics as well as the pendulum for a given controller. First we will demonstrate how to plot and animate our system without a control law.

from pendulum import pendulum_system
import numpy as np
from matplotlib import pyplot as plt

def controller(x):
    return 0

# Get the vector field
x = np.linspace(-2*np.pi, 2*np.pi, 400)
y = np.linspace(-10, 10, 400)
X, Y = np.meshgrid(x, y)
U, V = np.zeros(X.shape), np.zeros(Y.shape)
for i in range(X.shape[0]):
    for j in range(X.shape[1]):
        dxdt = pendulum_system([X[i, j], Y[i, j]], 0, controller)
        U[i, j], V[i, j] = dxdt

plt.streamplot(X, Y, U, V, color='k')
plt.xlabel(r'$x_1$')
plt.ylabel(r'$x_2$')
plt.title('Pendulum Phase Portrait')
plt.grid()
plt.show()

Simulations/Animations

We can also use this 'pendulum.py' code to simulate the pendulum

from pendulum import simulate_pendulum, make_animation
from IPython.display import HTML

def controller(x):
    u = 0
    return u

# Setup
x0 = [-np.pi/2.2, 0]    # initial conditions.
tfinal = 25.0       # Final time. Simulation time = 0 to tfinal.
Nt = 751           # Number of time steps

# Animation
theta_vec, thetadot_vec, dt = simulate_pendulum(x0, tfinal, Nt, controller)
anim = make_animation(dt, theta_vec, thetadot_vec)
plt.close()
HTML(anim.to_jshtml())

Once Loop Reflect

Adding Control

We can add a control law to stabilize the pendulum to the upright angle: $$ u(x) = \frac{g}{l}\sin(x_1) - K_p (x_1 - \pi) - K_d x_2 $$ for any $K_p$ and $K_d$ such that the following matrix results in eigenvalues with negative real parts: $$ A = \begin{bmatrix} 0 & 1 \\ -K_p & -K_d \end{bmatrix} $$

from pendulum import get_params
def controller(x):
    L,g = get_params()
    Kp = 1.0
    Kd = 2.0
    u = g/L*np.sin(x[0]) - Kp*(x[0] - np.pi) - Kd*x[1]
    return u

# Animation
theta_vec, thetadot_vec, dt = simulate_pendulum(x0, tfinal, Nt, controller)
anim = make_animation(dt, theta_vec, thetadot_vec)
plt.close()
HTML(anim.to_jshtml())

Once Loop Reflect

Gain Tuning

We can further play with the gains to change the behavior of the pendulum. For example, we will get faster convergence for $K_p = 1000$ and $K_d = 200$ as shown below.

from pendulum import get_params
def controller(x):
    L,g = get_params()
    Kp = 1000
    Kd = 200
    u = g/L*np.sin(x[0]) - Kp*(x[0] - np.pi) - Kd*x[1]
    return u

# Animation
theta_vec, thetadot_vec, dt = simulate_pendulum(x0, tfinal, Nt, controller)
anim = make_animation(dt, theta_vec, thetadot_vec)
plt.close()
HTML(anim.to_jshtml())

Once Loop Reflect